#include "wheel_group.h"

#define PI 3.14159265

rm_motor_groupTypeDef wheel_group_rudder_motor = {};
rm_motor_groupTypeDef wheel_group_driving_motor = {};

pid_controller wheel_group_rudder_speed_pid[2] = {};
//pid_controller wheel_group_rudder_angle_pid[4] = {};
pid_controller wheel_group_driving_speed_pid[2] = {};
	
float wheel_group_rudder_speed_target[2] = {};
float wheel_group_driving_speed_target[2] = {};
	
wheel_group_dataTypeDef wheel_group_data[2] = {};
int16_t wheel_group_zero_angle[2] = {0x0AA0, 0x1A9E};

void wheel_group_init()
{
	rm_motor_init(&wheel_group_rudder_motor, 0x1FF, &hcan2);
	rm_motor_init(&wheel_group_driving_motor, 0x200, &hcan2);
	
	rm_motor_cmd_max_config(&wheel_group_rudder_motor, 25000, 25000, 25000, 25000);
	rm_motor_cmd_max_config(&wheel_group_driving_motor, 16384, 16384, 16384, 16384);
	
	for (int i = 0; i < 2; i++)
	{
		pid_controller_create(wheel_group_rudder_speed_pid + i, 1500, 0, 0, 1000, 25000);//2500
		pid_controller_create(wheel_group_driving_speed_pid + i, 100, 0, 0, 1000, 16384);
	}
}

float wheel_group_angle_normal(float angle)
{
	if(angle > PI)
	{
		while(angle > PI)
		{
			angle -= (2 * PI);
		}
	}
	else if (angle < - PI)
	{
		while (angle < - PI)
		{
			angle += (2 * PI);
		}
	}
	return angle;
}

void wheel_group_data_refresh()
{
	float angle[2] = {};
	for (int i = 0; i <2; i++)
	{
		angle[i] = ((int16_t)wheel_group_rudder_motor.motor_data[i].angle - wheel_group_zero_angle[i]) * 2 * PI / 8192;
		wheel_group_data[i].angle = - wheel_group_angle_normal(angle[i]);
		wheel_group_data[i].rudder_speed = wheel_group_rudder_motor.motor_data[i].speed * 2 * PI / 60;
		wheel_group_data[i].driving_speed = wheel_group_driving_motor.motor_data[i].speed * 2 * PI / 60;
	}
}

void wheel_group_controller_execute()
{
	wheel_group_data_refresh();
	for (int i = 0; i < 2; i++)
	{
		pid_error_input(wheel_group_rudder_speed_pid + i, wheel_group_rudder_speed_target[i] - wheel_group_data[i].rudder_speed);
		pid_error_input(wheel_group_driving_speed_pid + i, wheel_group_driving_speed_target[i] - wheel_group_data[i].driving_speed);
	}
	rm_motor_cmd_refresh(&wheel_group_rudder_motor, wheel_group_rudder_speed_pid[0].output,
																									wheel_group_rudder_speed_pid[1].output,
																									0,
																									0);
	rm_motor_cmd_refresh(&wheel_group_driving_motor, 	wheel_group_driving_speed_pid[0].output,
																										wheel_group_driving_speed_pid[1].output,
																										0,
																										0);
	
	rm_motor_send_msg(&wheel_group_rudder_motor);
	rm_motor_send_msg(&wheel_group_driving_motor);
}

void wheel_group_rudder_callback(CAN_RxHeaderTypeDef* rx_header, uint8_t* data)
{
	rm_motor_receive_msg(&wheel_group_rudder_motor, rx_header, data);
}

void wheel_group_driving_callback(CAN_RxHeaderTypeDef* rx_header, uint8_t* data)
{
	rm_motor_receive_msg(&wheel_group_driving_motor, rx_header, data);
}